/***************************************************************************
*   Copyright (C) 2010-2011 by swkyer <swkyer@gmail.com>                  *
*                                                                         *
*   This program is free software; you can redistribute it and/or modify  *
*   it under the terms of the GNU General Public License as published by  *
*   the Free Software Foundation; either version 2 of the License, or     *
*   (at your option) any later version.                                   *
*                                                                         *
*   This program is distributed in the hope that it will be useful,       *
*   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
*   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
*   GNU General Public License for more details.                          *
*                                                                         *
*   You should have received a copy of the GNU General Public License     *
*   along with this program; if not, write to the                         *
*   Free Software Foundation, Inc.,                                       *
*   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
***************************************************************************/
#include "stdafx.h"
#include "Ejtag.h"


JLINKMIPS_API ejtag_t *ejtag_init(int jlinkdev)
{
	int retv;
	uint32_t mask;
	ejtag_t *pejtag;
	
	pejtag = (ejtag_t *)malloc(sizeof(ejtag_t));
	if (pejtag == NULL)
		return NULL;
	
	pejtag->jlinkdev = jlinkdev;
	pejtag->usbdev = jlinkusb_open(jlinkdev);
	if (pejtag->usbdev == NULL)
	{
		free(pejtag);
		return NULL;
	}

	pejtag->ir_len = EJTAG_INSTRUCTION_LENGTH;
	pejtag->dr_len = EJTAG_REGISTER_LENGTH;
	pejtag->state = TAP_INVALID;
	pejtag->trst_time = EJTAG_TRST_PULSE_MS;
	pejtag->rst_time = EJTAG_RST_PULSE_MS;

	pejtag->jacc.numbits = 0;
	pejtag->jacc.tdi = (uint8_t *)malloc(EJTAG_MAX_BIT_STREAM_LENGTH/sizeof(uint8_t));
	if (pejtag->jacc.tdi == NULL)
	{
		free(pejtag);
		return NULL;
	}
	pejtag->jacc.tms = (uint8_t *)malloc(EJTAG_MAX_BIT_STREAM_LENGTH/sizeof(uint8_t));
	if (pejtag->jacc.tms == NULL)
	{
		free(pejtag->jacc.tdi);
		free(pejtag);
		return NULL;
	}
	pejtag->jacc.tdo = (uint8_t *)malloc(EJTAG_MAX_BIT_STREAM_LENGTH/sizeof(uint8_t));
	if (pejtag->jacc.tdo == NULL)
	{
		free(pejtag->jacc.tms);
		free(pejtag->jacc.tdi);
		free(pejtag);
		return NULL;
	}

	retv = jlinkusb_get_caps(pejtag->usbdev, &pejtag->caps);
	if (retv != 0)
	{
		fprintf(stderr, "faild to read caps...\n");
		ejtag_exit(pejtag);
		return NULL;
	}
	if (pejtag->caps.caps & EMU_CAP_GET_CPU_CAPS)
	{
		retv = jlinkusb_get_caps_ex(pejtag->usbdev, &pejtag->caps);
		if (retv != 0)
		{
			fprintf(stderr, "faild to read caps ex...\n");
			ejtag_exit(pejtag);
			return NULL;
		}
	}
	if (pejtag->caps.caps & EMU_CAP_SPEED_INFO)
	{
		retv = jlinkusb_get_speed(pejtag->usbdev, &pejtag->speed);
		if (retv != 0)
		{
			fprintf(stderr, "faild to get speed...\n");
			ejtag_exit(pejtag);
			return NULL;
		}
	}
	if (pejtag->caps.caps & EMU_CAP_EX_GET_HW_INFO)
	{
		retv = jlinkusb_get_hw_info(pejtag->usbdev, &pejtag->hwinfo);
		if (retv != 0)
		{
			fprintf(stderr, "faild to get hardware info...\n");
			ejtag_exit(pejtag);
			return NULL;
		}
	}
	retv = jlinkusb_get_state(pejtag->usbdev, &pejtag->pinstate);
	if (retv != 0)
	{
		fprintf(stderr, "faild to get pin status...\n");
		ejtag_exit(pejtag);
		return NULL;
	}
	retv = jlinkusb_get_unknow_09(pejtag->usbdev, &pejtag->u09, 0);
	if (retv != 0)
	{
		fprintf(stderr, "faild to get unknow 09 CMD...\n");
		ejtag_exit(pejtag);
		return NULL;
	}
	retv = jlinkusb_get_unknow_09(pejtag->usbdev, &pejtag->u09, 1);
	if (retv != 0)
	{
		fprintf(stderr, "faild to get unknow 09 CMD...\n");
		ejtag_exit(pejtag);
		return NULL;
	}

	retv = jlinkusb_select_if(pejtag->usbdev, 0xFF, &mask);
	if (retv != 0)
	{
		fprintf(stderr, "faild to get interface...\n");
		ejtag_exit(pejtag);
		return NULL;
	}
	if (mask & TIF_JTAG)
	{
		retv = jlinkusb_select_if(pejtag->usbdev, 0x00, &mask);
		if (retv != 0)
		{
			fprintf(stderr, "faild to set interface 0x00...\n");
			ejtag_exit(pejtag);
			return NULL;
		}
	}
	else
	{
		fprintf(stderr, "DO NOT support JTAG...\n");
		ejtag_exit(pejtag);
		return NULL;
	}

	return pejtag;
}

JLINKMIPS_API void ejtag_exit(ejtag_t *pejtag)
{
	if (pejtag != NULL)
	{
		if (pejtag->usbdev != NULL)
			jlinkusb_close(pejtag->usbdev);

		free(pejtag->jacc.tms);
		free(pejtag->jacc.tdi);
		free(pejtag->jacc.tdo);
		free(pejtag);
	}
}

JLINKMIPS_API ejtag_t *ejtag_reinit(ejtag_t *pejtag)
{
	int jlinkdev = pejtag==NULL?0:pejtag->jlinkdev;

	ejtag_exit(pejtag);
	return ejtag_init(jlinkdev);
}

JLINKMIPS_API int ejtag_is_target_connect(ejtag_t *pejtag)
{
    int retv;

    if (pejtag == NULL)
        return -1;

    retv = jlinkusb_get_state(pejtag->usbdev, &pejtag->pinstate);
    if (retv != 0)
    {
        fprintf(stderr, "faild to get pin status...\n");
        return -2;
    }

    // FIXME
    if (pejtag->pinstate.voltage <= 1000)
        return 0;
    return 1;
}

JLINKMIPS_API int ejtag_trst_activate(ejtag_t *pejtag)
{
	int retv;

	if (pejtag == NULL)
		return -1;

	retv = jlinkusb_hw_trst0(pejtag->usbdev);
	if (retv != 0)
		return retv;
	pejtag->state = TAP_INVALID;
	return 0;
}

JLINKMIPS_API int ejtag_trst_deactivate(ejtag_t *pejtag)
{
	int retv;

	if (pejtag == NULL)
		return -1;

	retv = jlinkusb_hw_trst1(pejtag->usbdev);
	if (retv != 0)
		return retv;
	pejtag->state = TAP_RESET;
	return 0;
}

JLINKMIPS_API int ejtag_rst_activate(ejtag_t *pejtag)
{
	if (pejtag == NULL)
		return -1;

	return jlinkusb_hw_reset0(pejtag->usbdev);
}

JLINKMIPS_API int ejtag_rst_deactivate(ejtag_t *pejtag)
{
	if (pejtag == NULL)
		return -1;

	return jlinkusb_hw_reset1(pejtag->usbdev);
}

static const uint8_t bitvals[8] = {0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80};
static const uint8_t bitvals_rev[8] = {0xfe, 0xfd, 0xfb, 0xf7, 0xef, 0xdf, 0xbf, 0x7f};
static void ejtag_jacc_build(jlink_jtag_acc_t *pacc, int tdi, int tms)
{
	int curbyt = pacc->numbits / sizeof(uint8_t);
	int curbit = pacc->numbits % sizeof(uint8_t);
	if (tms)
		pacc->tms[curbyt] |= bitvals[curbit];
	else
		pacc->tms[curbyt] &= bitvals_rev[curbit];
	if (tdi)
		pacc->tdi[curbyt] |= bitvals[curbit];
	else
		pacc->tdi[curbyt] &= bitvals_rev[curbit];
	pacc->numbits++;
}

static int ejtag_jacc_debuild(jlink_jtag_acc_t *pacc)
{
	int curbyt = pacc->numbits / sizeof(uint8_t);
	int curbit = pacc->numbits % sizeof(uint8_t);
	pacc->numbits++;
	return (pacc->tdo[curbyt] & bitvals[curbit]) ? 1 : 0;
}

JLINKMIPS_API int ejtag_tap_reset(ejtag_t *pejtag, int use_trst)
{
	int retv;

	if (pejtag == NULL)
		return -1;

	if (use_trst)
	{
		ejtag_trst_activate(pejtag);
		Sleep(pejtag->trst_time);
		ejtag_trst_deactivate(pejtag);

		pejtag->state = TAP_RESET;
		return 0;
	}

	// TMS=1, 5 times from anywhere to TAP_RESET
	pejtag->jacc.numbits = 0;
	ejtag_jacc_build(&pejtag->jacc, 1, 1);
	ejtag_jacc_build(&pejtag->jacc, 1, 1);
	ejtag_jacc_build(&pejtag->jacc, 1, 1);
	ejtag_jacc_build(&pejtag->jacc, 1, 1);
	ejtag_jacc_build(&pejtag->jacc, 1, 1);

	retv = jlinkusb_hw_jtag3(pejtag->usbdev, &pejtag->jacc);
	if (retv != 0)
		return retv;

	pejtag->state = TAP_RESET;
	return 0;
}

JLINKMIPS_API int ejtag_runtest_idle(ejtag_t *pejtag)
{
	int retv;

	if (pejtag == NULL)
		return -1;

	// build the tms/tdi bit stream, from TAP_RESET to TAP_IDLE
	pejtag->jacc.numbits = 0;
	if (pejtag->state != TAP_RESET)
	{
		ejtag_jacc_build(&pejtag->jacc, 1, 1);
		ejtag_jacc_build(&pejtag->jacc, 1, 1);
		ejtag_jacc_build(&pejtag->jacc, 1, 1);
		ejtag_jacc_build(&pejtag->jacc, 1, 1);
		ejtag_jacc_build(&pejtag->jacc, 1, 1);
	}
	ejtag_jacc_build(&pejtag->jacc, 1, 0);

	retv = jlinkusb_hw_jtag3(pejtag->usbdev, &pejtag->jacc);
	if (retv != 0)
		return retv;

	pejtag->state = TAP_IDLE;
	return 0;
}

JLINKMIPS_API int ejtag_instruction(ejtag_t *pejtag, ubase_t ir_out, ubase_t *pirin)
{
	int i, retv;
	ubase_t irin = 0;

	if (pejtag == NULL)
		return -1;
	
	if (pejtag->state != TAP_IDLE)
		ejtag_runtest_idle(pejtag);

	// build the tms/tdi bit stream
	pejtag->jacc.numbits = 0;
	ejtag_jacc_build(&pejtag->jacc, 1, 1);				// TAP_DRSELECT
	ejtag_jacc_build(&pejtag->jacc, 1, 1);				// TAP_IRSELECT
	ejtag_jacc_build(&pejtag->jacc, 1, 0);				// TAP_IRCAPTURE
	ejtag_jacc_build(&pejtag->jacc, 1, 0);				// TAP_IRSHIFT
	for (i=0; i<pejtag->ir_len-1; i++)
	{
		if (ir_out & 0x01)
			ejtag_jacc_build(&pejtag->jacc, 1, 0);
		else
			ejtag_jacc_build(&pejtag->jacc, 0, 0);
		ir_out >>= 1;
	}
	if (ir_out & 0x01)
		ejtag_jacc_build(&pejtag->jacc, 1, 1);			// TAP_IREXIT1
	else
		ejtag_jacc_build(&pejtag->jacc, 0, 1);
	ejtag_jacc_build(&pejtag->jacc, 1, 1);				// TAP_IRUPDATE
	ejtag_jacc_build(&pejtag->jacc, 1, 0);				// TAP_IDLE

	retv = jlinkusb_hw_jtag3(pejtag->usbdev, &pejtag->jacc);
	if (retv != 0)
		return retv;

	// get the return value
	if (pirin)
	{
		pejtag->jacc.numbits = 0;
		ejtag_jacc_debuild(&pejtag->jacc);				// TAP_DRSELECT
		ejtag_jacc_debuild(&pejtag->jacc);				// TAP_IRSELECT
		ejtag_jacc_debuild(&pejtag->jacc);				// TAP_IRCAPTURE
		ejtag_jacc_debuild(&pejtag->jacc);				// TAP_IRSHIFT
		for (i=0; i<pejtag->ir_len; i++)
		{
			if (ejtag_jacc_debuild(&pejtag->jacc))
				irin |= (1<<i);
		}
		*pirin = irin;
	}

	pejtag->state = TAP_IDLE;
	return 0;
}

JLINKMIPS_API int ejtag_data(ejtag_t *pejtag, ubase_t data_out, ubase_t *pdatain)
{
	int i, retv;
	ubase_t datain = 0;

	if (pejtag == NULL)
		return -1;

	if (pejtag->state != TAP_IDLE)
		ejtag_runtest_idle(pejtag);

	// build the tms/tdi bit stream
	pejtag->jacc.numbits = 0;
	ejtag_jacc_build(&pejtag->jacc, 1, 1);				// TAP_DRSELECT
	ejtag_jacc_build(&pejtag->jacc, 1, 0);				// TAP_DRCAPTURE
	ejtag_jacc_build(&pejtag->jacc, 1, 0);				// TAP_DRSHIFT
	for (i=0; i<pejtag->dr_len-1; i++)
	{
		if (data_out & 0x01)
			ejtag_jacc_build(&pejtag->jacc, 1, 0);
		else
			ejtag_jacc_build(&pejtag->jacc, 0, 0);
		data_out >>= 1;
	}
	if (data_out & 0x01)
		ejtag_jacc_build(&pejtag->jacc, 1, 1);			// TAP_DREXIT1
	else
		ejtag_jacc_build(&pejtag->jacc, 0, 1);
	ejtag_jacc_build(&pejtag->jacc, 1, 1);				// TAP_DRUPDATE
	ejtag_jacc_build(&pejtag->jacc, 1, 0);				// TAP_IDLE

	retv = jlinkusb_hw_jtag3(pejtag->usbdev, &pejtag->jacc);
	if (retv != 0)
		return retv;

	// get the return value
	if (pdatain)
	{
		pejtag->jacc.numbits = 0;
		ejtag_jacc_debuild(&pejtag->jacc);				// TAP_DRSELECT
		ejtag_jacc_debuild(&pejtag->jacc);				// TAP_DRCAPTURE
		ejtag_jacc_debuild(&pejtag->jacc);				// TAP_DRSHIFT
		for (i=0; i<pejtag->dr_len; i++)
		{
			if (ejtag_jacc_debuild(&pejtag->jacc))
				datain |= (1<<i);
		}
		*pdatain = datain;
	}

	pejtag->state = TAP_IDLE;
	return 0;
}

JLINKMIPS_API int ejtag_get_idcode(ejtag_t *pejtag, uint32_t *idcode)
{
	int retv;

	if (pejtag == NULL || idcode == NULL)
		return -1;

	retv = ejtag_instruction(pejtag, JTAG_REGISTER_IDCODE, NULL);
	if (retv != 0)
		return retv;
	return ejtag_data(pejtag, 0xFFFFFFFF, idcode);
}

JLINKMIPS_API int ejtag_get_impcode(ejtag_t *pejtag, uint32_t *impcode)
{
	int retv;

	if (pejtag == NULL || impcode == NULL)
		return -1;

	retv = ejtag_instruction(pejtag, JTAG_REGISTER_IMPCODE, NULL);
	if (retv != 0)
		return retv;
	return ejtag_data(pejtag, 0xFFFFFFFF, impcode);
}
